function [ Y ] = lagrangeinterpolation(x, xi, Yi) %#codegen
%LAGRANGEINTERPOLATION 拉格朗日插值
%
% Input Arguments:
% # x: 标量，插值点的x坐标
% # xi: 长度为p的向量，已知数据点的x坐标向量
% # Yi: 最后一维尺寸为p的数组，已知数据点的y坐标数据
%
% Output Arguments:
% # Y: 数组，尺寸与Yi去掉最后一维后的尺寸相等，插值点的y坐标数据
%
% References:
% # 《应用导航算法工程基础》“拉格朗日插值函数”

% NOTE: 本函数使用线性索引
szYi = size(Yi);
sz = szYi(1, 1:(end-1));
if length(sz) == 1
    szY = [sz 1];
else
    szY = sz;
end
Y = zeros(szY);

for i=1:szYi(1, end)
    l = 1;
    for j=1:szYi(1, end)
        if j ~= i
            l = l * (x-xi(j)) / (xi(i)-xi(j)); % 计算拉格朗日基函数
        end
    end
    Y = Y + l*lastdimsubarray(Yi, i);
end
end